/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : api.c
 * Description        : MintaProtocol and CANOpen control these function to c
 *                      ontrol the actuator
 ******************************************************************************
 * @attention we must use "Call" as a starting point
 *
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "main.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/**
 * @brief  enable/disable actuator
 * @note
 * @param
 * @retval
 */
bool Call_MotorPower_Set(char enable)
{
  if (enable == true)
  {
    g_sys.State = STATE_INIT;

    return true;
  }
  else if (enable == false)
  {
    g_sys.State = STATE_STOP;
    return true;
  }
  else
    return false;
}

char Call_MotorPower_Get(void)
{
  return g_sys.State;
}
/**
 * @brief
 * @note
 * @param
 * @retval
 */
bool Call_MotorMode_Set(char mode)
{
  float save_position = g_sys.Para.ReadPosition;
  // clear parameters
  memset(&g_sys.Para, 0, sizeof(g_sys.Para));
  g_sys.Para.SetPosition = save_position;

  PID_Controller_Reset();
  // Trajectory_Parameters_Reset();

  g_sys.Mode = mode;
  return true;
}

char Call_MotorMode_Get(void)
{
  return g_sys.Mode;
}

bool Call_MotorIQ_Set(float set)
{
  // if it have brake,determine whether the BRAKE is open
  if (g_sys.RW.UserPara.Para.BrakeLevel > 0)
  {
    if (!BrakeState_Get())
      return false;
  }
  // if the set value is greater the maximum value
  if (fabs(set) > PHASE_I_MAXIMUM)
    return false;
  if (fabs(set) > g_sys.RW.UserPara.Para.IQLimit)
    return false;

  g_sys.Para.SetIQ = set;
  return true;
}

bool Call_MotorVelocity_Set(float set)
{
  // if it have brake,determine whether the BRAKE is open
  if (g_sys.RW.UserPara.Para.BrakeLevel > 0)
  {
    if (!BrakeState_Get())
      return false;
  }

  if (g_sys.RW.UserPara.Para.BrakeLevel > 0)
  {
    if (!BrakeState_Get())
      return false;
  }

  if (fabs(set) > MOTOR_SPEED_MAXIMUN)
    return false;

  if (fabs(set) > g_sys.RW.UserPara.Para.VelocityLimit)
    return false;

  float dError = g_sys.Para.SetVelocity - set;
  if (fabs(dError) > 600)
  {
    // phase step
    return false;
  }

  g_sys.Para.SetVelocity = set;
  return true;
}

bool Call_MotorPosition_Set(float set)
{
  // if it have brake,determine whether the BRAKE is open
  if (g_sys.RW.UserPara.Para.BrakeLevel > 1)
  {
    if (!BrakeState_Get())
      return false;
  }
  // make sure it doesn't exceed this value
  if (set > g_sys.RW.UserPara.Para.PositionUpper)
    return false;
  if (set < g_sys.RW.UserPara.Para.PostionLower)
    return false;

  g_sys.Para.SetPosition = set;
  if (g_sys.Mode == MODE_TPOSITION)
  {
    Trajectory_MoveCommand(g_sys.Para.ReadPosition, g_sys.Para.SetPosition);
  }

  return true;
}

float Call_MotorIQ_Get(void)
{
  return g_sys.Para.ReadIQ;
}

float Call_MotorVelocity_Get(void)
{
  return g_sys.Para.ReadVelocity;
}

float Call_MotorPosition_Get(void)
{
  return g_sys.Para.ReadPosition;
}
/*------------------------------ set pid --------------------------*/
bool Call_PIDCurrentKp_Set(float kp)
{
  g_sys.RW.UserPara.Para.Pid_Q.kp = kp;
  g_sys.RW.UserPara.Para.Pid_D.kp = kp;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDCurrentKi_Set(float ki)
{
  g_sys.RW.UserPara.Para.Pid_Q.ki = ki;
  g_sys.RW.UserPara.Para.Pid_D.ki = ki;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDCurrentUpper_Set(float upper)
{
  if ((upper > 1.0f) || (upper < 0.0f))
  {
    g_sys.RW.UserPara.Para.Pid_Q.upper = 0.0f;
    g_sys.RW.UserPara.Para.Pid_D.upper = 0.0f;
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_Q.upper = upper * PHASE_I_MAXIMUM;
    g_sys.RW.UserPara.Para.Pid_D.upper = upper * PHASE_I_MAXIMUM;
    PID_Parameters_Update();
    return true;
  }
}
bool Call_PIDCurrentLower_Set(float lower)
{
  if ((lower < -1.0f) || (lower > 0.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_Q.lower = lower * PHASE_I_MAXIMUM;
    g_sys.RW.UserPara.Para.Pid_D.lower = lower * PHASE_I_MAXIMUM;
    PID_Parameters_Update();
    return true;
  }
}
bool Call_PIDVelocityKp_Set(float kp)
{
  g_sys.RW.UserPara.Para.Pid_V.kp = kp;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDVelocityKi_Set(float ki)
{
  g_sys.RW.UserPara.Para.Pid_V.ki = ki;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDVelocityUpper_Set(float upper)
{
  if ((upper > 1.0f) || (upper < 0.0f))
  {
    g_sys.RW.UserPara.Para.Pid_V.upper = 0.0f;
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_V.upper = upper * PHASE_I_MAXIMUM;
    PID_Parameters_Update();
    return true;
  }
}
bool Call_PIDVelocityLower_Set(float lower)
{
  if ((lower < -1.0f) || (lower > 0.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_V.lower = lower * PHASE_I_MAXIMUM;
    PID_Parameters_Update();
    return true;
  }
}
bool Call_PIDPositionKp_Set(float kp)
{
  g_sys.RW.UserPara.Para.Pid_P.kp = kp;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDPositionKi_Set(float ki)
{
  g_sys.RW.UserPara.Para.Pid_P.ki = ki;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDPositionKd_Set(float kd)
{
  g_sys.RW.UserPara.Para.Pid_P.kd = kd;
  PID_Parameters_Update();
  return true;
}
bool Call_PIDPositionUpper_Set(float upper)
{
  if ((upper > 1.0f) || (upper < 0.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_P.upper = upper * MOTOR_SPEED_MAXIMUN;
    PID_Parameters_Update();
    return true;
  }
}
bool Call_PIDPositionLower_Set(float lower)
{
  if ((lower < -1.0f) || (lower > 0.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.Pid_P.lower = lower * MOTOR_SPEED_MAXIMUN;
    PID_Parameters_Update();
    return true;
  }
}
/*------------------------------ get pid --------------------------*/
float Call_PIDCurrentKp_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_Q.kp;
}
float Call_PIDCurrentKi_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_Q.ki;
}
float Call_PIDCurrentUpper_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_Q.upper / PHASE_I_MAXIMUM);
}
float Call_PIDCurrentLower_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_Q.lower / PHASE_I_MAXIMUM);
}
float Call_PIDVelocityKp_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_V.kp;
}
float Call_PIDVelocityKi_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_V.ki;
}
float Call_PIDVelocityUpper_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_V.upper / PHASE_I_MAXIMUM);
}
float Call_PIDVelocityLower_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_V.lower / PHASE_I_MAXIMUM);
}
float Call_PIDPositionKp_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_P.kp;
}
float Call_PIDPositionKi_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_P.ki;
}
float Call_PIDPositionKd_Get(void)
{
  return g_sys.RW.UserPara.Para.Pid_P.kd;
}
float Call_PIDPositionUpper_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_P.upper / MOTOR_SPEED_MAXIMUN);
}
float Call_PIDPositionLower_Get(void)
{
  return (g_sys.RW.UserPara.Para.Pid_P.lower / MOTOR_SPEED_MAXIMUN);
}
// input limit
bool Call_CurrentInputLimit_Set(float limit)
{
  if ((limit < -1.0f) || (limit > 1.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.IQLimit = limit * PHASE_I_MAXIMUM;
    return true;
  }
}
float Call_CurrentInputLimit_Get(void)
{
  return (g_sys.RW.UserPara.Para.IQLimit / PHASE_I_MAXIMUM);
}
bool Call_VelocityInputLimit_Set(float limit)
{
  if ((limit < -1.0f) || (limit > 1.0f))
  {
    return false;
  }
  else
  {
    g_sys.RW.UserPara.Para.VelocityLimit = limit * MOTOR_SPEED_MAXIMUN;
    return true;
  }
}
float Call_VelocityInputLimit_Get(void)
{
  return (g_sys.RW.UserPara.Para.VelocityLimit / MOTOR_SPEED_MAXIMUN);
}
bool Call_PositionInputUpper_Set(float upper)
{
  if (upper < 0)
    return false;
  else
  {
    g_sys.RW.UserPara.Para.PositionUpper = upper * TWO_PI;
    return true;
  }
}
float Call_PositionInputUpper_Get(void)
{
  return (g_sys.RW.UserPara.Para.PositionUpper / TWO_PI);
}
bool Call_PositionInputLower_Set(float lower)
{
  if (lower > 0)
    return false;
  else
  {
    g_sys.RW.UserPara.Para.PostionLower = lower * TWO_PI;
    return true;
  }
}
float Call_PositionInputLower_Get(void)
{
  return (g_sys.RW.UserPara.Para.PostionLower / TWO_PI);
}

// trapezoid
float Call_TPosVMAX_Get(void)
{
  return g_sys.RW.UserPara.Para.TP_VelocityLimit;
}

float Call_TPosAcc_Get(void)
{
  return g_sys.RW.UserPara.Para.TP_AccSpeed;
}

float Call_TPosDec_Get(void)
{
  return g_sys.RW.UserPara.Para.TP_DecSpeed;
}

void Call_TPosVMAX_Set(float set)
{
  g_sys.RW.UserPara.Para.TP_VelocityLimit = set;
  Trajectory_Parameters_Updata();
}

void Call_TPosAcc_Set(float set)
{
  g_sys.RW.UserPara.Para.TP_AccSpeed = set;
  Trajectory_Parameters_Updata();
}

void Call_TPosDec_Set(float set)
{
  g_sys.RW.UserPara.Para.TP_DecSpeed = set;
  Trajectory_Parameters_Updata();
}

float Call_TVelVMAX_Get(void)
{
  return g_sys.RW.UserPara.Para.TS_VelocityLimit;
}

float Call_TVelAcc_Get(void)
{
  return g_sys.RW.UserPara.Para.TS_AccSpeed;
}

float Call_TVelDec_Get(void)
{
  return g_sys.RW.UserPara.Para.TS_DecSpeed;
}

void Call_TVelVMAX_Set(float set)
{
  g_sys.RW.UserPara.Para.TS_VelocityLimit = set;
  Trajectory_Parameters_Updata();
}

void Call_TVelAcc_Set(float set)
{
  g_sys.RW.UserPara.Para.TS_AccSpeed = set;
  Trajectory_Parameters_Updata();
}

void Call_TVelDec_Set(float set)
{
  g_sys.RW.UserPara.Para.TS_DecSpeed = set;
  Trajectory_Parameters_Updata();
}

bool Call_UserParametersSave(void)
{
  bool return1;
  return1 = UserPara_UserParameters_Save();
  return return1;
}
// brake contorl
void Call_BrakeLevel_Set(uint8_t level)
{
  if ((level == 0) || (level == 1) || (level == 2) || (level == 3))
    g_sys.RW.UserPara.Para.BrakeLevel = level;
}
uint8_t Call_BrakeLevel_Read(void)
{
  return g_sys.RW.UserPara.Para.BrakeLevel;
}

uint8_t Call_BrakeState_Read(void)
{
  return BrakeState_Get();
}

char Call_BrakeEnable_Set(char enable)
{
  switch (g_sys.RW.UserPara.Para.BrakeLevel)
  {
  case 1:
    Brake_Control(enable);
    break;
  case 2:
  case 3:
    if ((g_sys.Mode == MODE_POSITION) || (g_sys.Mode == MODE_TPOSITION))
    {
      Brake_Control(enable);
    }
    break;
  case 0:
  default:
    break;
  }
  return BrakeState_Get();
}

/* USER CODE END Application */
